%%  Simulation

clear all;close all;clc;

for j=1:6
    xSF(:,j)=[0.5 -0.5];
    nSF(j) =norm(xSF(:,j)); 
end

n=2;m=1;l=1;dM=4;dm=2;
R2=1*eye(m);R1=1*eye(l);
Alpha=1;Beta=1;
JSF=0;

eps=0.08;r=1;
m1=1+eps;m1=sqrt(m1);
m2=(1+(eps^-1))*(3*r+2);m2=sqrt(m2);

% SECOND SYSTEM     Example 1 of MY THIRD PAPER  
el=0.08;
%first sub-system
A10=[1.4 0.1;-0.4 0.2];Ad10=[0 0.01;0.02 0];
B10=[-0.7;1];
E11=0.1*eye(n);F11=0.1*eye(n);G11=[0.1;0];
E11bar=el*E11;F11bar=el*F11;G11bar=el*G11;
F1=[0.01 0;0 0.01];H1=[0.002 0;0 0.002];
%second sub-system
A20=[1.1 0.2;0.3 0.4];Ad20=[0 0.01;0.02 0];
B20=[0.8;-0.5];
C2=[3 0.7];C1=C2;
E12=0.2*eye(n);F12=0.2*eye(n);G12=[0.2;0];
E12bar=el*E12;F12bar=el*F12;G12bar=el*G12;
F2=[0.01 0;0 0.01];H2=[0.002 0;0 0.002];

R1=C2'*R1*C2;

T0=dM+2;Tf=T0+40;

for i=T0:Tf
    
    d=[2*ones((Tf-T0)/4,1);3*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1);2*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1)];
    
    S1=sdpvar(n,n);S2=sdpvar(n,n);
    W1=sdpvar(n,n);W2=sdpvar(n,n);
    Y11=sdpvar(m,n,'full');Y12=sdpvar(m,n,'full');
    Y21=zeros(m,n);Y22=zeros(m,n);
    Landa1=sdpvar(1,1);Landa2=sdpvar(1,1);
    gama=sdpvar(1,1);
    
    C=[S1>=0,S2>=0,W1>=0,W2>=0];
    C=C+[Landa1>=0,Landa2>=0];
    
    %offline LMI for dm=2
    
    %LMI#1      j=1;s=1;
    C=C+[[-S1 zeros(n) zeros(n,(dM-1)*n) m1*(A10*S1+B10*Y11)' Y11'*R2 S1 sqrt(dM-1)*S1 S1*R1 (m2*E11bar*S1)' (m2*G11bar*Y11)' (m2*Alpha*F1*S1)' zeros(n,3*n) zeros(n,(dM-dm)*n);...
        zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad10*W1+B10*Y21)' Y21'*R2 zeros(n,6*n) (m2*Beta*H1*W1)' (m2*F11bar*W1)' (m2*G11bar*Y21)' zeros(n,(dM-dm)*n);...
        zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),9*n) blkdiag(W2,W2);...
        zeros(n*(dm-1),2*n) zeros(n,n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),9*n) zeros(n,(dM-dm)*n);...
        m1*(A10*S1+B10*Y11) m1*(Ad10*W1+B10*Y21) zeros(n,(dM-1)*n) -S1 zeros(n,m) zeros(n,11*n);...
        R2*Y11 R2*Y21 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,11*n);...
        S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n,10*n);
        sqrt(dM-1)*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,9*n);...
        R1*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -gama*eye(n) zeros(n,8*n);...
        m2*E11bar*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) -S1 zeros(n,7*n);...
        m2*G11bar*Y11 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,4*n) -S1 zeros(n,6*n);...
        m2*Alpha*F1*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,5*n) -Landa1*eye(n) zeros(n,5*n);...
        zeros(n) m2*Beta*H1*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,6*n) -Landa1*eye(n) zeros(n,4*n);...
        zeros(n) m2*F11bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,7*n) -S1 zeros(n,3*n);...
        zeros(n) m2*G11bar*Y21 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,8*n) -S1 zeros(n,2*n);...
        zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,9*n) blkdiag(-W1,-W1)]<=0];
    
    %LMI#1      j=1;s=2;
    C=C+[[-S1 zeros(n) zeros(n,(dM-1)*n) m1*(A10*S1+B10*Y11)' Y11'*R2 S1 sqrt(dM-1)*S1 S1*R1 (m2*E11bar*S1)' (m2*G11bar*Y11)' (m2*Alpha*F1*S1)' zeros(n,3*n) zeros(n,(dM-dm)*n);...
        zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad10*W1+B10*Y21)' Y21'*R2 zeros(n,6*n) (m2*Beta*H1*W1)' (m2*F11bar*W1)' (m2*G11bar*Y21)' zeros(n,(dM-dm)*n);...
        zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),9*n) blkdiag(W2,W2);...
        zeros(n*(dm-1),2*n) zeros(n,n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),9*n) zeros(n,(dM-dm)*n);...
        m1*(A10*S1+B10*Y11) m1*(Ad10*W1+B10*Y21) zeros(n,(dM-1)*n) -S2 zeros(n,m) zeros(n,11*n);...
        R2*Y11 R2*Y21 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,11*n);...
        S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n,10*n);
        sqrt(dM-1)*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,9*n);...
        R1*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -gama*eye(n) zeros(n,8*n);...
        m2*E11bar*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) -S2 zeros(n,7*n);...
        m2*G11bar*Y11 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,4*n) -S2 zeros(n,6*n);...
        m2*Alpha*F1*S1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,5*n) -Landa2*eye(n) zeros(n,5*n);...
        zeros(n) m2*Beta*H1*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,6*n) -Landa2*eye(n) zeros(n,4*n);...
        zeros(n) m2*F11bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,7*n) -S2 zeros(n,3*n);...
        zeros(n) m2*G11bar*Y21 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,8*n) -S2 zeros(n,2*n);...
        zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,9*n) blkdiag(-W1,-W1)]<=0];
    
    %LMI#1      j=2;s=1;
    C=C+[[-S2 zeros(n) zeros(n,(dM-1)*n) m1*(A20*S2+B20*Y12)' Y12'*R2 S2 sqrt(dM-1)*S2 S2*R1 (m2*E12bar*S2)' (m2*G12bar*Y12)' (m2*Alpha*F2*S2)' zeros(n,3*n) zeros(n,(dM-dm)*n);...
        zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad20*W1+B20*Y22)' Y22'*R2 zeros(n,6*n) (m2*Beta*H2*W1)' (m2*F12bar*W1)' (m2*G12bar*Y22)' zeros(n,(dM-dm)*n);...
        zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),9*n) blkdiag(W2,W2);...
        zeros(n*(dm-1),2*n) zeros(n,n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),9*n) zeros(n,(dM-dm)*n);...
        m1*(A20*S2+B20*Y12) m1*(Ad20*W1+B20*Y22) zeros(n,(dM-1)*n) -S1 zeros(n,m) zeros(n,11*n);...
        R2*Y12 R2*Y22 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,11*n);...
        S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n,10*n);
        sqrt(dM-1)*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,9*n);...
        R1*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -gama*eye(n) zeros(n,8*n);...
        m2*E12bar*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) -S1 zeros(n,7*n);...
        m2*G12bar*Y12 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,4*n) -S1 zeros(n,6*n);...
        m2*Alpha*F2*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,5*n) -Landa1*eye(n) zeros(n,5*n);...
        zeros(n) m2*Beta*H2*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,6*n) -Landa1*eye(n) zeros(n,4*n);...
        zeros(n) m2*F12bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,7*n) -S1 zeros(n,3*n);...
        zeros(n) m2*G12bar*Y22 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,8*n) -S1 zeros(n,2*n);...
        zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,9*n) blkdiag(-W1,-W1)]<=0];
    
    %LMI#1      j=2;s=2;
    C=C+[[-S2 zeros(n) zeros(n,(dM-1)*n) m1*(A20*S2+B20*Y12)' Y12'*R2 S2 sqrt(dM-1)*S2 S2*R1 (m2*E12bar*S2)' (m2*G12bar*Y12)' (m2*Alpha*F2*S2)' zeros(n,3*n) zeros(n,(dM-dm)*n);...
        zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad20*W1+B20*Y22)' Y22'*R2 zeros(n,6*n) (m2*Beta*H2*W1)' (m2*F12bar*W1)' (m2*G12bar*Y22)' zeros(n,(dM-dm)*n);...
        zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),9*n) blkdiag(W2,W2);...
        zeros(n*(dm-1),2*n) zeros(n,n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),9*n) zeros(n,(dM-dm)*n);...
        m1*(A20*S2+B20*Y12) m1*(Ad20*W1+B20*Y22) zeros(n,(dM-1)*n) -S2 zeros(n,m) zeros(n,11*n);...
        R2*Y12 R2*Y22 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,11*n);...
        S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n,10*n);
        sqrt(dM-1)*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,9*n);...
        R1*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -gama*eye(n) zeros(n,8*n);...
        m2*E12bar*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) -S2 zeros(n,7*n);...
        m2*G12bar*Y12 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,4*n) -S2 zeros(n,6*n);...
        m2*Alpha*F2*S2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,5*n) -Landa2*eye(n) zeros(n,5*n);...
        zeros(n) m2*Beta*H2*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,6*n) -Landa2*eye(n) zeros(n,4*n);...
        zeros(n) m2*F12bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,7*n) -S2 zeros(n,3*n);...
        zeros(n) m2*G12bar*Y22 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,8*n) -S2 zeros(n,2*n);...
        zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,9*n) blkdiag(-W1,-W1)]<=0];
    
    %LMI#2  j=1;
    C=C+[[1 xSF(:,i-1)' xSF(:,i-2)' xSF(:,i-3)' xSF(:,i-4)' xSF(:,i-5)' sqrt(dM-1)*xSF(:,i-2)' sqrt(dM-2)*xSF(:,i-3)' sqrt(dM-3)*xSF(:,i-4)';...
        xSF(:,i-1) S1 zeros(n,7*n);...
        xSF(:,i-2) zeros(n) W1 zeros(n,6*n);...
        xSF(:,i-3) zeros(n,2*n) W1 zeros(n,5*n);...
        xSF(:,i-4) zeros(n,3*n) W1 zeros(n,4*n);...
        xSF(:,i-5) zeros(n,4*n) W1 zeros(n,3*n);...
        sqrt(dM-1)*xSF(:,i-2) zeros(n,5*n) W2 zeros(n,2*n);...
        sqrt(dM-2)*xSF(:,i-3) zeros(n,6*n) W2 zeros(n);...
        sqrt(dM-3)*xSF(:,i-4) zeros(n,7*n) W2]>=0];

    %LMI#2  j=2;
    C=C+[[1 xSF(:,i-1)' xSF(:,i-2)' xSF(:,i-3)' xSF(:,i-4)' xSF(:,i-5)' sqrt(dM-1)*xSF(:,i-2)' sqrt(dM-2)*xSF(:,i-3)' sqrt(dM-3)*xSF(:,i-4)';...
        xSF(:,i-1) S2 zeros(n,7*n);...
        xSF(:,i-2) zeros(n) W1 zeros(n,6*n);...
        xSF(:,i-3) zeros(n,2*n) W1 zeros(n,5*n);...
        xSF(:,i-4) zeros(n,3*n) W1 zeros(n,4*n);...
        xSF(:,i-5) zeros(n,4*n) W1 zeros(n,3*n);...
        sqrt(dM-1)*xSF(:,i-2) zeros(n,5*n) W2 zeros(n,2*n);...
        sqrt(dM-2)*xSF(:,i-3) zeros(n,6*n) W2 zeros(n);...
        sqrt(dM-3)*xSF(:,i-4) zeros(n,7*n) W2]>=0];
    
    %LMI#3  s=1,s=2;
    C=C+[S1-Landa1*eye(n)>=0, S2-Landa2*eye(n)>=0];
    
    o=gama;
    op=sdpsettings('solver','lmilab,*');
    diag=optimize(C,gama,op)
    
    S1v=value(S1);S2v=value(S2);
    W1v=value(W1);
    W2v=value(W2);
    Landa1v=value(Landa1);Landa2v=value(Landa2);
    Y11v=value(Y11);Y12v=value(Y12);
    Y21v=value(Y21);Y22v=value(Y22);
    K1v=Y11v*inv(S1v);
    K2v=Y12v*inv(S2v);
    gamav=value(gama);
    Gamma(i)=gamav;
    
    delta=el*sin(i);
    z=mod(i,2);
    if (z>0)
        xSF(:,i)=(A10+delta*E11)*xSF(:,i-1)+(Ad10+delta*F11)*xSF(:,i-1-d(i))+(B10+delta*G11)*K1v*xSF(:,i-1)+[0.01*sin(xSF(1,i-1));0]+[0.002*sin(xSF(1,i-1-d(i)));0];
        ySF(:,i-1)=C1*xSF(:,i-1);
        uSF(:,i-1)=K1v*xSF(:,i-1);
        nSF(i) =norm(xSF(:,i));
        sSF(i)=1;
        VSF(i-1)=xSF(:,i-1)'*gamav*inv(S1v)*xSF(:,i-1);
        for k=-1:-1:-dM
            VSF(i-1)=VSF(i-1)+xSF(:,i-1+k)'*gamav*inv(W1v)*xSF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VSF(i-1)=VSF(i-1)+(dM+k)*xSF(:,i-1+k)'*gamav*inv(W2v)*xSF(:,i-1+k);
        end
        VSF(i-1);
    else
        xSF(:,i)=(A20+delta*E12)*xSF(:,i-1)+(Ad20+delta*F12)*xSF(:,i-1-d(i))+(B20+delta*G12)*K2v*xSF(:,i-1)+[0.01*sin(xSF(1,i-1));0]+[0.002*sin(xSF(1,i-1-d(i)));0];
        ySF(:,i-1)=C2*xSF(:,i-1);
        uSF(:,i-1)=K2v*xSF(:,i-1);
        nSF(i) =norm(xSF(:,i));
        sSF(i)=2;
        VSF(i-1)=xSF(:,i-1)'*gamav*inv(S2v)*xSF(:,i-1);
        for k=-1:-1:-dM
            VSF(i-1)=VSF(i-1)+xSF(:,i-1+k)'*gamav*inv(W1v)*xSF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VSF(i-1)=VSF(i-1)+(dM+k)*xSF(:,i-1+k)'*gamav*inv(W2v)*xSF(:,i-1+k);
        end
        VSF(i-1);
    end
    JSF=JSF+ySF(:,i-1)'*eye(l)*ySF(:,i-1)+uSF(:,i-1)'*eye(m)*uSF(:,i-1);
end

save('RMPC_SF.mat')
CostFunction_kx_InequalQ=JSF;

uSF1max=max(uSF(1,T0-1:Tf-1));
uSF1min=min(uSF(1,T0-1:Tf-1));
uSF1Max=max(abs(uSF1min),abs(uSF1max));

xSF1max=max(xSF(1,T0:Tf-1));
xSF1min=min(xSF(1,T0:Tf-1));
xSF1Max=max(abs(xSF1min),abs(xSF1max));

xSF2max=max(xSF(2,T0:Tf-1));
xSF2min=min(xSF(2,T0:Tf-1));
xSF2Max=max(abs(xSF2min),abs(xSF2max));

ySFmax=max(ySF(1,T0:Tf-1));
ySFmin=min(ySF(1,T0:Tf-1));
ySFMax=max(abs(ySFmin),abs(ySFmax));

VSFMax=max(VSF(T0-1:Tf-1));

List={};
Result_table = table(dm,dM,CostFunction_kx_InequalQ,uSF1Max,ySFMax,gamav,VSFMax,'RowNames',List)

t=0:Tf-T0;
figure;stairs(t,xSF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('States');legend('x_1','x_2')
%figure;stairs(t,sSF(T0:Tf),'LineWidth',2);grid on;xlabel('k (sample)');ylabel('Switching signal');ylim([0 3])
figure;stairs(t,uSF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Control Signal (u)');
figure;stairs(t,ySF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Output (y)');
%figure;plot(1:50,d,'LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('d(k)');
figure;stairs(t,VSF(T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('V');
%figure;plot(t,nSF(T0:Tf)','*');grid on;xlabel('k (sample)');ylabel('Norm of state variables');